#include <iostream>
#include <cmath>

const double wheelDiameter = 0.1;
const double axleLength = 0.5;
const double pi = 3.1415926;

// 机器人当前的位置和方向
double x = 0.0,y = 0.0 ,theta = 0.0;

// 控制周期单位:秒
double dt = 0.01;

void updatePosition(double leftWheelSpeed,double rightWheelSpeed){
    double linearVelocity = (leftWheelSpeed+rightWheelSpeed) / 2.0;
    double angularVelocity = (rightWheelSpeed - leftWheelSpeed) / axleLength;

    x+=linearVelocity*cos(theta)*dt;
    y+=linearVelocity*sin(theta)*dt;
    theta += angularVelocity*dt;
}

double targetX = 1.0;
double targetY = 1.0;

double kP_linear = 0.5;
double kP_angular = 1.0;

int main(){
    for(int i=0;i<1000;i++){
        double distance = sqrt(pow(targetX-x,2)+pow(targetY - y,2));
        double angle = atan2(targetY - y,targetX -x)
    }

// 非线性的拟合手段
// theta//
// tf变换-->本质还是质点运动
// theta-->
// -->odom,imu

// 连续轨迹控制--\theta // 还得额外控
// 
// 迪卡尔-->非线性的跟踪手段

// cmu学校 --导航和探索